

#include <std.h>
#include <stdio.h>
#include <string.h>
#include <log.h>
#include <tsk.h>
#include <ecm.h>
#include <gio.h>
#include "ti/pspiom/i2c/I2c.h"
#include "..\libMath\matrix.h"

#include "BMA180.h"
#include "common.h"
#include "stdio.h"
#include "math.h"
#include "common_i2c.h"
#include "navconbioscfg.h"
#include "err.h"

//The unit before and after compensation is 'g'
double acc_4g_caltp_coe[12] = {	3.38505e-07,	5.39394e-07,	1.69372e-06,
								2.61790e-07,	4.41890e-05,	1.00628e-04,
								-4.41224e-05,	8.38426e-04,	1.71401e-03,
								-1.61158e-03,	4.95514e-03,	1.30259e-02		};

double acc_4g_cal_coe[12] = {		0.995875,   -0.0100396,   	5.38481e-03,
									0.0138450,   0.981132,    	0.0166295,
									2.65684e-03, 1.69674e-03,  	0.988372,
									9.45978e-04, 4.83449e-02,  	-7.18977e-03	};

/* ========================================================================== */
/*                           FUNCTION DEFINITIONS                             */
/* ========================================================================== */

/* \fn
 *
 * \brief   Function to BMA180 accelerometer driver.
 *
 */

//read register values start from address addr, length provided by user
int BMA180_READ_REG(GIO_Handle inhd,GIO_Handle outhd, Uint8 REG, Uint8 *buf, size_t len)
{
	int error = 0;
	//request to read register @ address *addr
	error = i2cWrite(outhd,BMA180_ADDR,( unsigned char *)&REG,1,I2c_DEFAULT_WRITE);
	if(error!=IOM_COMPLETED){
		return error;
	}
	//perform read, write to user-defined buffer
    error = i2cRead(inhd,BMA180_ADDR,( unsigned char *)buf,len,I2c_DEFAULT_READ);
	if(error!=IOM_COMPLETED){
		return error;
	}
	return error;
}

int BMA180_WRITE_REG(GIO_Handle inhd,GIO_Handle outhd, Uint8 REG,Uint8 value, op_mode op)
{
	int error = 0;
	Uint8 temp[2];

	if((REG >= BMA180_BWTCS) && (REG <= BMA180_CRC))	//unlock writing to regs from 20H to 3BH
	{
		error = BMA180_UNLOCK(inhd,outhd);
		if(error){
			return error;
		}
	}
	// Avoid soft reset
	if(REG != 	BMA180_SOFTRST){
		temp[0] = REG;
		temp[1] = value;
		error = i2cWrite(outhd,BMA180_ADDR,( unsigned char *)temp,2,I2c_DEFAULT_WRITE);
		if(error!=IOM_COMPLETED){
		 	if((REG >= BMA180_BWTCS) && (REG <= BMA180_CRC))	//unlock writing to regs from 20H to 3BH
			{
				BMA180_LOCK(inhd,outhd);
			}
			return error;
		}
		if(op == VALIDATE){
			error = BMA180_READ_REG(inhd,outhd,REG,temp,1);
			if(error){
				//nop
			}else if(temp[0] != value){
				error = IO_VALIDATED_ERR;
			}
		}
	}

 	if((REG >= BMA180_BWTCS) && (REG <= BMA180_CRC))	//unlock writing to regs from 20H to 3BH
	{
 		error = BMA180_LOCK(inhd,outhd);
	}
 	return error;
}


//Connect to BMA180, read ID and identify, return INVALID_ID if ID mismatch
int BMA180_CONNECT(GIO_Handle inhd,GIO_Handle outhd){

	Uint8 temp[2];
	int error = IOM_COMPLETED;

	temp[0] = BMA180_ID;
	error = BMA180_READ_REG(inhd,outhd,BMA180_ID,temp,1);
	if(error){
		return error;
	}
	if (temp[0] != 0x03) {
		error = INVALID_ID;
    }else{
    	error = 0;
    }
	return error;
}

int BMA180_UNLOCK(GIO_Handle inhd,GIO_Handle outhd)
{
	Uint8 temp[2];
	int error = IOM_COMPLETED;

	//read CTRLREG0
	error = BMA180_READ_REG(inhd,outhd,BMA180_CTRLREG0,temp,1);
	if(error){
		return IO_LCK_FAIL;
	}
	//unlock
    temp[1] = temp[0] | 0x10;
	temp[0] = BMA180_CTRLREG0;
	//write back to CTRLREG0
	error = i2cWrite(outhd,BMA180_ADDR,( unsigned char *)temp,2,I2c_DEFAULT_WRITE);
	if(error!=IOM_COMPLETED){
		return IO_LCK_FAIL;
	}
	return error;
}

int BMA180_LOCK(GIO_Handle inhd,GIO_Handle outhd)
{
	Uint8 temp[2];
	int error = IOM_COMPLETED;

	//read CTRLREG0
	error = BMA180_READ_REG(inhd,outhd,BMA180_CTRLREG0,temp,1);
	if(error){
		return IO_LCK_FAIL;
	}
	//lock
    temp[1] = temp[0] & 0xEF;
	temp[0] = BMA180_CTRLREG0;
	//write back to CTRLREG0
	error = i2cWrite(outhd,BMA180_ADDR,( unsigned char *)temp,2,I2c_DEFAULT_WRITE);
	if(error!=IOM_COMPLETED){
		return IO_LCK_FAIL;
	}
	return error;
}

/*Set the range of operation, and wait for 1/(2*bandwidth)
 * second before output is stabilized.
 **/
int BMA180_SET_RANGE(GIO_Handle inhd,GIO_Handle outhd, Uint8 range)
{
	Uint8 temp[2];
	int error = 0;

	error = BMA180_READ_REG(inhd,outhd,BMA180_OFTLSB1,temp,1);
	if(error){
		return error;
	}

	temp[1] = temp[0] & 0xF1;					//clear out
	temp[1] = temp[1] + (range << 1);			//set range
	temp[0] = BMA180_OFTLSB1;
	//write back to BMA180_OFTLSB1, require validation upon write
	error = BMA180_WRITE_REG(inhd,outhd,BMA180_OFTLSB1,temp[1],VALIDATE);
	//Add 1/(2*BW) sec delay here
	TSK_sleep(4); //@150HzBW = 3.33ms, 4 SYSTEM CLOCK
	return error;
}

//Set BW - bandwidth
int BMA180_SET_BW(GIO_Handle inhd,GIO_Handle outhd, Uint8 bandwith)
{
	Uint8 temp[2];
	int error = 0;

	error = BMA180_READ_REG(inhd,outhd,BMA180_BWTCS,temp,1);
	if(error){
		return error;
	}

	//set bandwidth
	temp[1] = temp[0] & 0x0F;					//clear out
	temp[1] = temp[1] + (bandwith << 4);

	//update register value
	error = BMA180_WRITE_REG(inhd,outhd,BMA180_BWTCS,temp[1],VALIDATE);
	//Add 1000us delay
	TSK_sleep(1);
	//C6747_waitusec(10);
	return error;
}

int BMA180_SOFTRESET(GIO_Handle inhd,GIO_Handle outhd)
{
	Uint8 temp[2];
	int error = 0;
	temp[0] = BMA180_SOFTRST;
	temp[1] = BMA180_RESET_VAL;
	error = i2cWrite(outhd,BMA180_ADDR,( unsigned char *)temp,2,I2c_DEFAULT_WRITE);
	if(error!=IOM_COMPLETED){
		return IO_RESET_ERR;
	}
	//sleep for 1 system clock
	TSK_sleep(1);
	//C6747_waitusec(10);		//must wait for 10us after reset
	return error;
}

void BMA180_SCALE_DATA(short*rdata, double *data,int acc_scale)
{
	Int16 x,y,z;

    x = rdata[0];
    data[0] = (double)x * acc_scale / 16384.0;

    y = rdata[1];
    data[1] = (double)y * acc_scale / 16384.0;

    z = rdata[2];
    data[2] = (double)z * acc_scale / 16384.0;

    data[3] = ((double)((Int8)rdata[3] + 128)) * 0.5 - 40;

}


int BMA180_GET_RDATA(GIO_Handle inhd,GIO_Handle outhd,short *data)
{
	Uint8 temp[8];
	Int16 x,y,z;
	int error = IOM_COMPLETED;

	error = BMA180_READ_REG(inhd, outhd, BMA180_ACCXLSB,temp, 7);
	if(error!=IOM_COMPLETED){
		return error;
	}
    x = (((Int16)temp[1]) << 8) | temp[0];
    x >>= 2;
    data[0] = x;

    y = (((Int16)temp[3]) << 8) | temp[2];
    y >>= 2;
    data[1] = y;

    z = (((Int16)temp[5]) << 8) | temp[4];
    z >>= 2;
    data[2] = z;

    data[3] = (short)temp[6];

    return error;
}


double BMA180_GETPITCH(double x,double y,double z)
//Normalization procedure is deleted now
//Please normalise the measurements from acc&mag firstly when calling get_pitch&roll functions
{
	double pitch = 0;
	//double roll = 0;
	//double Gp,Gpx,Gpy,Gpz;

	//AN3461 Page10
	/*
	Gp = (double)sqrt(x * x + y * y + z * z);
    Gpx = (double)x / Gp;
    Gpy = (double)y / Gp;
    Gpz = (double)z / Gp;
	pitch = atan(-Gpx / sqrt(Gpy * Gpy + Gpz * Gpz)) * 180 / PI;
	if((Gpy == 0) && (Gpz == 0) && (-Gpx > 0))
		pitch = 90;
	if((Gpy == 0) && (Gpz == 0) && (-Gpx < 0))
		pitch = -90;
	*/
	pitch = atan(-x / sqrt(y*y + z*z));

	if((y == 0)&&(z==0)&&(-x>0))
		pitch = 90 * D2R;
	else if((y == 0)&&(z==0)&&(-x<0))
		pitch = -90 * D2R;

	return pitch;
}

double BMA180_GETROLL(double x,double y,double z)
{
	double roll = 0;
	//double Gp,Gpx,Gpy,Gpz;

	//AN3461 Page10
	/*
	Gp = (double)sqrt(x * x + y * y + z * z);
    Gpx = (double)x / Gp;
    Gpy = (double)y / Gp;
    Gpz = (double)z / Gp;
    (void)Gpx;	//avoid warning
	roll = atan2(-Gpy,-Gpz) * 180 / PI;
	if((-Gpz ==0) && (-Gpy > 0))
		roll = 90;
	if((-Gpz == 0) && (-Gpy < 0))
		roll = -90;
	*/
	roll = atan2(y,z);
	return roll;
}

void BMA180_CAL_DATA(short*rdata, double *data, int acc_scale, Uint8 mode)
//rdata-raw data, data-output 3*1, acc_scale, mode
// mode is used for select whether temperature compensation is applied, 0-no temperature compensation, 1-temperature compensated
{
	Int16 x,y,z;
	double temp[4],cal_data[3],delt_tp[4],cal_tp_data[3],temp1[3];

    x = rdata[0];
    temp[0] = (double)x * acc_scale / 16384.0;

    y = rdata[1];
    temp[1] = (double)y * acc_scale / 16384.0;

    z = rdata[2];
    temp[2] = (double)z * acc_scale / 16384.0;
    temp[3] = 1;

    data[3] = ((double)((Int8)rdata[3] + 128)) * 0.5 - 40;
    //start compensation
    if(mode == 0)
    {
    	MatMul(1,4,3, temp, acc_4g_cal_coe, cal_data);
    	data[0] = cal_data[0];
    	data[1] = cal_data[1];
    	data[2] = cal_data[2];
    }
    else if(mode == 1)
    {
    	delt_tp[3] = 1;
    	delt_tp[2] = data[3] - 35;
    	delt_tp[1] = delt_tp[2] * delt_tp[2];
    	delt_tp[0] = pow((delt_tp[2]),3);
    	MatMul(1,4,3, temp, acc_4g_cal_coe, cal_data);
    	MatMul(1,4,3, delt_tp, acc_4g_caltp_coe, temp1);
    	temp1[0] = -temp1[0];	//multiply -1 with temp1
    	temp1[1] = -temp1[1];
    	temp1[2] = -temp1[2];
    	MatAdd(1, 3, cal_data, temp1, cal_tp_data);
    	data[0] = cal_tp_data[0];
    	data[1] = cal_tp_data[1];
    	data[2] = cal_tp_data[2];
    }
}


/* ========================================================================== */
/*                                END OF FILE                                 */
/* ========================================================================== */
